"""
有一个三轴机械臂,由三个180°舵机控制。设定坐标系,X轴Y轴平行于地面,Z轴垂直于地面。
1号舵机控制底盘以上绕Z轴旋转,设定1号舵机旋转角度为∠α。旋转点为点A。
2号舵机控制其上部分绕X轴旋转，设定2号舵机旋转角度为∠β。旋转点为点B。
3号舵机控制末端执行器绕X轴旋转，设定3号舵机旋转角度为∠γ。旋转点为点C。
AB点直线距离为M，BC点直线距离为N，C点到执行器终点距离为L。
已知M，N，L长度，给出终点坐标，求三个舵机旋转角度。
"""

import math
import numpy as np

def calculate_angles(x, y, z, M, N, L):
    """
    计算三个舵机的旋转角度
    
    参数:
    x, y, z: 目标点坐标
    M: AB间距离
    N: BC间距离
    L: C点到末端执行器距离
    
    返回:
    (alpha, beta, gamma): 三个舵机的角度(弧度)
    """
    # 1. 计算alpha (1号舵机角度)
    alpha = math.atan2(y, x)
    
    # 2. 将终点坐标投影到XZ平面
    r = math.sqrt(x*x + y*y)  # 在XY平面上的投影长度
    
    # 3. 使用余弦定理计算beta和gamma
    # 首先计算从A点到终点的距离
    D = math.sqrt(r*r + z*z)
    
    # 计算从A点到终点与水平面的夹角
    phi = math.atan2(z, r)
    
    # 使用余弦定理计算beta
    cos_beta = (M*M + D*D - (N+L)*(N+L)) / (2*M*D)
    # 防止浮点数计算误差导致的域错误
    cos_beta = min(1, max(-1, cos_beta))
    beta = phi + math.acos(cos_beta)
    
    # 计算gamma
    # 首先计算BC延长线与水平面的夹角
    theta = math.atan2(z - M*math.sin(beta), r - M*math.cos(beta))
    gamma = theta - beta
    
    # 将角度规范化到[0, pi]范围内
    alpha = normalize_angle(alpha)
    beta = normalize_angle(beta)
    gamma = normalize_angle(gamma)
    
    return (alpha, beta, gamma)

def normalize_angle(angle):
    """
    将角度规范化到[0, pi]范围内
    """
    while angle < 0:
        angle += math.pi
    while angle > math.pi:
        angle -= math.pi
    return angle

# 示例使用
if __name__ == "__main__":
    # 假设目标坐标（单位：毫米）
    x = 50
    y = 50
    z = 100
    
    # 机械臂参数
    M = 100  # AB长度
    N = 80   # BC长度
    L = 50   # C到末端长度
    
    try:
        # 计算角度
        alpha, beta, gamma = calculate_angles(x, y, z, M, N, L)
        
        # 转换为角度制
        alpha_deg = math.degrees(alpha)
        beta_deg = math.degrees(beta)
        gamma_deg = math.degrees(gamma)
        
        print(f"1号舵机角度(α): {alpha_deg:.2f}°")
        print(f"2号舵机角度(β): {beta_deg:.2f}°")
        print(f"3号舵机角度(γ): {gamma_deg:.2f}°")
        
    except ValueError as e:
        print(f"错误：目标点超出机械臂工作范围！")